#include "Control.h"

// 全局变量定义
PID_Params PID = {0};
PID_State angle_state = {0,0,0,0};
PID_State speed_state_l = {0,0,0,0};
PID_State speed_state_r = {0,0,0,0};
PID_State turn_state = {0,0,0,0};  // 新增转向环状态

// 灰度传感器数据定义（在其他模块更新）
float GraySensors[GRAY_SENSOR_COUNT] = {0};

// 角度环PID计算
float Angle_PID(void) {
    return Calculate_PID(PID.Median, SAngle.Angle[2], 
                        &angle_state, 
                        PID.Bp, PID.Bi, PID.Bd);
}

// 左轮速度环PID计算
float SpeedL_Pwm(void) {
    return Calculate_PID(PID.Target_Speed, Encoder.countl,
                        &speed_state_l,
                        PID.Sp_L, PID.Si_L, PID.Sd_L);
}

// 右轮速度环PID计算
float SpeedR_Pwm(void) {
    return Calculate_PID(PID.Target_Speed, Encoder.countr,
                        &speed_state_r,
                        PID.Sp_R, PID.Si_R, PID.Sd_R);
}
/*
// 新增：转向环PID计算（基于灰度传感器）
float Turn_PID(void) {
    // 计算转向输出（目标位置为0表示居中）
    return Calculate_PID(0.0f, xunji(), 
                        &turn_state, 
                        PID.Tp, PID.Ti, PID.Td);
}*/

#define MIN_ACTIVE_SENSORS   1     // 最小有效传感器数
#define MAX_ERROR           28.0f  // 最大误差值


/********************************************
函数功能：差速环返回值
参数：  无
返回值：差速值（左轮速度为middle+x，右轮速度为middle-x）
********************************************/
float Turn_PID(void) {
    // 1. 当多个传感器同时检测到黑线时，使用加权平均算法
    float weighted_sum = 0.0f;
    int active_count = 0;
    float Error = 5.0f; // 默认误差值
    
    // 传感器权重配置
    const float weights[8] = {
        -52.0f,  // x1 (最左侧)
        -30.0f,  // x2
        -18.0f,  // x3
        -6.0f,   // x4
        6.0f,    // x5
        18.0f,   // x6
        30.0f,   // x7
        52.0f    // x8 (最右侧)
    };
    
    // 检测传感器状态并计算加权和
    if (Huiduo.x1 == 0) { weighted_sum += weights[0]; active_count++; }
    if (Huiduo.x2 == 0) { weighted_sum += weights[1]; active_count++; }
    if (Huiduo.x3 == 0) { weighted_sum += weights[2]; active_count++; }
    if (Huiduo.x4 == 0) { weighted_sum += weights[3]; active_count++; }
    if (Huiduo.x5 == 0) { weighted_sum += weights[4]; active_count++; }
    if (Huiduo.x6 == 0) { weighted_sum += weights[5]; active_count++; }
    if (Huiduo.x7 == 0) { weighted_sum += weights[6]; active_count++; }
    if (Huiduo.x8 == 0) { weighted_sum += weights[7]; active_count++; }
    
    // 3. 处理检测到多个黑线的情况
    if (active_count > 0) {
        
        // 新增：在特定计数器状态下处理多重检测
        if (active_count > 2) { // 多个传感器同时检测到黑线
            if (counter.count == 1) {
                // 往左偏了，取负值
                Error = -50.0f;
            } 
            else if (counter.count == 3) {
                // 往右偏了，取正值
                Error = 50.0f;
            }
            else {
                // 其他计数状态使用加权平均值
                Error = weighted_sum / active_count;
            }
        } 
        else {
            // 单一检测使用加权平均值
            Error = weighted_sum / active_count;
        }
        
        // 计算并返回PID值
        return Calculate_PID(0.0f, Error, 
                        &turn_state, 
                        PID.Tp, PID.Ti, PID.Td);
    }
    
    // 4. 没有检测到任何黑线
    return 0;
}



// PID参数设置（添加转向环参数）
void Get_Pid(float Bp, float Bd, float Bi, float Target_Speed,
             float Sp_L, float Sd_L, float Si_L,
             float Sp_R, float Sd_R, float Si_R,
             float Tp, float Ti, float Td) 
{
    //PID.Median = B_Median;
    PID.Target_Speed = Target_Speed;
    PID.Bp = Bp;
    PID.Bd = Bd;
    PID.Bi = Bi;
    PID.Sp_L = Sp_L;
    PID.Sd_L = Sd_L;
    PID.Si_L = Si_L;
    PID.Sp_R = Sp_R;
    PID.Sd_R = Sd_R;
    PID.Si_R = Si_R;
    PID.Tp = Tp;  // 转向环比例参数
    PID.Ti = Ti;  // 转向环积分参数
    PID.Td = Td;  // 转向环微分参数
}

